
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <stdint.h>
#include <iostream>
#include <ios>
#include <sstream>
#include <iomanip>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>
#include "rclcpp/executor.hpp"
#include <img_msgs/msg/h26_x_frame.hpp>
#include <std_msgs/msg/string.hpp>
#include <nav_msgs/msg/odometry.hpp> // ROS2标准定义的里程计消息
#include <geometry_msgs/msg/twist.h>
#include <originbot_msgs/msg/originbot_status.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sys/socket.h>
#include <std_msgs/msg/detail/header__struct.hpp>

#include <google/protobuf/message_lite.h>

#include "ros.pb.h"
#include "h26x_frame.pb.h"
#include "Odometry.pb.h"
#include "origin_bot_status.pb.h"
#include "device_auth.pb.h"
#include "LaserScan.pb.h"
#include "tcp_server.h"

extern int main_tcp_server(int port);
extern void main_tcp_server_close();
extern void tcp_broadcast(const std::string &message);

const char *kDelimiter = "$$panxuesen$$"; // 分隔符

static bool first = true;
void save_protobuf(std::string data)
{
    std::fstream file("h26x.data", std::ios::out);
    file << data;
    file.close();
}

void hexdump(const char* data, int length) {
    std::ios_base::fmtflags flags = std::cout.flags();
    int i = 0;
    for (int j = 0; j < length; j++) {
        std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)(unsigned char)data[j];
        i++;
        if (i % 16 == 0) {
            std::cout << std::endl;
        } else {
            std::cout << " ";
        }
    }
    std::cout.flags(flags);
}

void protobuf2string(google::protobuf::MessageLite *msg, ROS2_MsgType type, std::string &out)
{
    std::string out2;
    msg->SerializeToString(&out2);
    ROS2 ros2;
    ros2.set_msg_type(type);
    ros2.set_msg_data(out2);
    ros2.SerializeToString(&out);
    out += kDelimiter;
}

void tcp_send(google::protobuf::MessageLite *msg, ROS2_MsgType type)
{
    std::string out;
    protobuf2string(msg, type, out);
    // RCLCPP_INFO(this->get_logger(), "send len:%d", out2.size());
    // hexdump(out2.data(), out2.size());
    tcp_broadcast(out);
}

class PhoneSubscriber : public rclcpp::Node
{
public:
    PhoneSubscriber()
        : Node("phone_subscriber")
    {
        rclcpp::SensorDataQoS qos;
        pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
        sub_ = this->create_subscription<img_msgs::msg::H26XFrame>("/image_raw/compressed/h265", qos, std::bind(&PhoneSubscriber::img_callback, this, std::placeholders::_1));
        this->radar_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>("scan", qos, std::bind(&PhoneSubscriber::radar_listener_callback, this, std::placeholders::_1));
        this->odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "odom", 10, std::bind(&PhoneSubscriber::odom_listener_callback, this, std::placeholders::_1));
        this->status_sub_ = this->create_subscription<originbot_msgs::msg::OriginbotStatus>(
            "originbot_status", 10, std::bind(&PhoneSubscriber::status_listener_callback, this, std::placeholders::_1));
        timer_ = create_wall_timer(
            std::chrono::seconds(1), std::bind(&PhoneSubscriber::timer_callback, this));
        num_ = 0;
    }

    void setTwist(double linear, double angular)
    {
        geometry_msgs::msg::Twist msg;
        msg.linear.x = linear;
        msg.linear.y = 0;
        msg.linear.z = 0;
        msg.angular.x = 0;
        msg.angular.y = 0;
        msg.angular.z = angular;
        pub_->publish(msg);
    }

private:

    void img_callback(const img_msgs::msg::H26XFrame::SharedPtr msg)
    {
        static uint32_t all_size = 0;
        num_++;
        H26XFrame_Time *dts = new H26XFrame_Time();
        H26XFrame_Time *pts = new H26XFrame_Time();
        H26XFrame frame;
        frame.set_index(msg->index);
        std::string encoding((const char *)msg->encoding.data(), msg->encoding.size());
        frame.add_encoding(encoding);
        frame.set_height(msg->height);
        frame.set_width(msg->width);
        dts->set_sec(msg->dts.sec);
        dts->set_nanosec(msg->dts.nanosec);
        pts->set_sec(msg->pts.sec);
        pts->set_nanosec(msg->pts.nanosec);
        frame.set_allocated_dts(dts);
        frame.set_allocated_pts(pts);
        std::string data((const char *)msg->data.data(), msg->data.size());
        frame.set_data(data);
        // frame.release_dts();
        // frame.release_pts();
        tcp_send(&frame, ROS2::MsgType::ROS2_MsgType_H26X_FRAME);
    }

    void HeaderMsg2Proto(const std_msgs::msg::Header *msg_header, ros2::proto::std_msgs_Header *proto_header)
    {
        ros2::proto::builtin_interfaces_Time *stamp = new ros2::proto::builtin_interfaces_Time();
        stamp->set_sec(msg_header->stamp.sec);
        stamp->set_nanosec(msg_header->stamp.nanosec);
        proto_header->set_allocated_stamp(stamp);
        proto_header->set_frame_id(msg_header->frame_id);
    }

    void Msg2Proto(const nav_msgs::msg::Odometry::SharedPtr msg, ros2::proto::Odometry &odom)
    {
        using namespace ros2::proto;
        ros2::proto::std_msgs_Header *header = new ros2::proto::std_msgs_Header();
        ros2::proto::builtin_interfaces_Time *stamp = new ros2::proto::builtin_interfaces_Time();
        geometry_msgs_PoseWithCovariance *pose = new geometry_msgs_PoseWithCovariance();
        geometry_msgs_Pose *pose1 = new geometry_msgs_Pose();
        geometry_msgs_Quaternion *orientation = new geometry_msgs_Quaternion();
        geometry_msgs_Point *point = new geometry_msgs_Point();
        geometry_msgs_TwistWithCovariance *twist = new geometry_msgs_TwistWithCovariance();
        geometry_msgs_Twist *twist1 = new geometry_msgs_Twist();
        geometry_msgs_Vector3 *angular = new geometry_msgs_Vector3();
        geometry_msgs_Vector3 *linear = new geometry_msgs_Vector3();
        stamp->set_sec(msg->header.stamp.sec);
        stamp->set_nanosec(msg->header.stamp.nanosec);
        header->set_allocated_stamp(stamp);
        header->set_frame_id(msg->header.frame_id);
        orientation->set_x(msg->pose.pose.orientation.x);
        orientation->set_y(msg->pose.pose.orientation.y);
        orientation->set_z(msg->pose.pose.orientation.z);
        orientation->set_w(msg->pose.pose.orientation.w);
        pose1->set_allocated_orientation(orientation);
        point->set_x(msg->pose.pose.position.x);
        point->set_y(msg->pose.pose.position.y);
        point->set_z(msg->pose.pose.position.z);
        pose1->set_allocated_position(point);
        pose->set_allocated_pose(pose1);
        for (int i = 0; i < msg->pose.covariance.size(); i++)
        {
            pose->add_covariance(msg->pose.covariance[i]);
        }
        angular->set_x(msg->twist.twist.angular.x);
        angular->set_y(msg->twist.twist.angular.y);
        angular->set_z(msg->twist.twist.angular.z);
        twist1->set_allocated_angular(angular);
        linear->set_x(msg->twist.twist.linear.x);
        linear->set_y(msg->twist.twist.linear.y);
        linear->set_z(msg->twist.twist.linear.z);
        twist1->set_allocated_linear(linear);
        twist->set_allocated_twist(twist1);
        for (int i = 0; i < msg->twist.covariance.size(); i++)
        {
            twist->add_covariance(msg->twist.covariance[i]);
        }
        odom.set_allocated_header(header);
        odom.set_child_frame_id(msg->child_frame_id);
        odom.set_allocated_pose(pose);
        odom.set_allocated_twist(twist);
    }

    void LaserMsg2Proto(const sensor_msgs::msg::LaserScan::SharedPtr msg, ros2::proto::LaserScan &laser)
    {
        using namespace ros2::proto;
        ros2::proto::std_msgs_Header *header = new ros2::proto::std_msgs_Header();
        HeaderMsg2Proto(&msg->header, header);
        laser.set_allocated_header(header);
        laser.set_angle_min(msg->angle_min);
        laser.set_angle_max(msg->angle_max);
        laser.set_angle_increment(msg->angle_increment);
        laser.set_time_increment(msg->time_increment);
        laser.set_scan_time(msg->scan_time);
        laser.set_range_min(msg->range_min);
        laser.set_range_max(msg->range_max);
        for (const auto& range : msg->ranges) {
            laser.add_ranges(range);
        }
        for (const auto& intensity : msg->intensities) {
            laser.add_intensities(intensity);
        }
    }

    void odom_listener_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        ros2::proto::Odometry odom;
        Msg2Proto(msg, odom);
        tcp_send(&odom, ROS2::MsgType::ROS2_MsgType_ODO_METRY);
        // RCLCPP_INFO(this->get_logger(), "Robot Position: \"%0.2f, %0.2f\"",
        //     msg->pose.pose.position.x, msg->pose.pose.position.y);
    }
    int calculateBatteryPercentage(float voltage)
    {
        // Assume the battery has a fully charged voltage of 12.6V and a fully depleted voltage of 9.6V
        int maxVoltage = 126;
        int minVoltage = 105; // 3.5v
        voltage *= 10;
        if (voltage > maxVoltage)
        {
            return 100;
        }
        else if (voltage < minVoltage)
        {
            return 0;
        }
        else
        {
            return ((int)voltage - minVoltage) * 100 / (maxVoltage - minVoltage);
        }
    }
    void status_listener_callback(const originbot_msgs::msg::OriginbotStatus::SharedPtr msg)
    {
        OriginBotStatus status;
        status.set_battery_voltage(msg->battery_voltage);
        status.set_buzzer_on(msg->buzzer_on);
        status.set_led_on(msg->led_on);
        status.set_battery_precent(calculateBatteryPercentage(msg->battery_voltage));
        tcp_send(&status, ROS2::MsgType::ROS2_MsgType_ORIGIN_BOT_STATUS);
    }
    void radar_listener_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        /*
            header：消息的头部，包含了时间戳和坐标系信息。
            angle_min：激光雷达的最小测量角度（弧度制）。
            angle_max：激光雷达的最大测量角度（弧度制）。
            angle_increment：激光雷达测量角度之间的间隔（弧度制）。
            time_increment：每两个测量之间的时间间隔（秒）。
            scan_time：每次测量所用的时间（秒）。
            range_min：激光雷达的最小测量距离（米）。
            range_max：激光雷达的最大测量距离（米）。
            ranges：激光雷达测量得到的距离数组（米）。
        */
        ros2::proto::LaserScan laser;
        LaserMsg2Proto(msg, laser);
        tcp_send(&laser, ROS2::MsgType::ROS2_MsgType_LASER_SCAN);
        RCLCPP_INFO(this->get_logger(), "scan_time:%0.2f range_min:%0.2f range_max:%0.2f range len:%d", msg->scan_time, msg->range_min, msg->range_max, msg->ranges.size());
    }
    void timer_callback()
    {
        static int count = 0;
        if (0 == num_)
        {
            count++;
            if (5 == count)
            {
                RCLCPP_WARN(this->get_logger(), "mipi cam may not work current, restart it");
                system("/etc/systemd/system/originbot.service.d/restart_mipicam.sh &");
                count = 0;
            }
        }
        else
        {
            count = 0;
        }
        RCLCPP_INFO(this->get_logger(), "img fps %d", num_);
        num_ = 0;
    }
    uint8_t num_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
    rclcpp::Subscription<img_msgs::msg::H26XFrame>::SharedPtr sub_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<originbot_msgs::msg::OriginbotStatus>::SharedPtr status_sub_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr radar_sub_;
    rclcpp::TimerBase::SharedPtr timer_;
    ros2::proto::Odometry odom;
};

// 创建节点
static std::shared_ptr<PhoneSubscriber> node = nullptr;
void processPacket(int fd, void *buffer, int len)
{
    ROS2 ros2;
    bool res = ros2.ParseFromArray(buffer, len);
    if (!res)
    {
        RCLCPP_ERROR(node->get_logger(), "ros2.ParseFromArray failed, len:%d\n", len);
        return;
    }
    switch (ros2.msg_type())
    {
    case ROS2_MsgType::ROS2_MsgType_CMD_VEL_PUBLISH:
    {
        ros2::proto::geometry_msgs_Twist twist;
        res = twist.ParseFromString(ros2.msg_data());
        if (!res)
        {
            RCLCPP_ERROR(node->get_logger(), "ROS2_MsgType_CMD_VEL_PUBLISH parse failed, len:%d\n", len);
            return;
        }
        RCLCPP_INFO(node->get_logger(), "linear:%0.2f twist:%0.2f", twist.linear().x(), twist.angular().z());
        node->setTwist(twist.linear().x(), twist.angular().z());
    }
    break;

    case ROS2_MsgType::ROS2_MsgType_DEVICE_AUTH:
    {
        DeviceAuth auth;
        bool auth_ok = false;
        res = auth.ParseFromString(ros2.msg_data());
        if (!res)
        {
            RCLCPP_ERROR(node->get_logger(), "ROS2_MsgType_DEVICE_AUTH parse failed, len:%d\n", len);
            return;
        }
        RCLCPP_INFO(node->get_logger(), "auth: user_name:%s user_passwd:%s", auth.user_name().c_str(), auth.user_passwd().c_str());
        if (auth.user_name() == "panxuesen" && auth.user_passwd() == "panxuesen")
        {
            auth_ok = true;
        }
        else if (auth.user_name() == "guchenglong" && auth.user_passwd() == "guchenglong")
        {
            auth_ok = true;
        }
        else
        {
            RCLCPP_INFO(node->get_logger(), "auth: user_name:%s not allowd", auth.user_name().c_str());
        }
        if (auth_ok)
        {
            if (g_client_manager.authClient(fd))
            {
                auth.set_auth_result(DeviceAuth_AuthResult::DeviceAuth_AuthResult_SUCCESS);
            }
            else
            {
                auth.set_auth_result(DeviceAuth_AuthResult::DeviceAuth_AuthResult_AUTH_FAILED);
            }
        }
        else
        {
            auth.set_auth_result(DeviceAuth_AuthResult::DeviceAuth_AuthResult_REJECT);
        }
        std::string out;
        protobuf2string(&auth, ROS2_MsgType::ROS2_MsgType_DEVICE_AUTH, out);
        send(fd, out.data(), out.size(), 0);
        if(!auth_ok)
        {
            g_client_manager.removeClient(fd);
        }
    }
    break;

    default:
        break;
    }
}

int main(int argc, char *argv[])
{
    system("ps -ef | grep 'ros2 run mipi_cam' | grep -v grep | awk '{print $2}' | xargs kill -9 2>/dev/null");
    rclcpp::init(argc, argv);
    node = std::make_shared<PhoneSubscriber>();
    /*
    // 创建执行器
    rclcpp::executors::MultiThreadedExecutor executor;
    // 添加节点的回调函数和定时器
    executor.add_node(node->get_node_base_interface());
    // 开始执行
    executor.spin();
    */

    auto future = std::async(std::launch::async, []()
                             { main_tcp_server(12345); });

    // rclcpp::spin_until_future_complete(node, future.share());
    rclcpp::spin(node);
    RCLCPP_INFO(node->get_logger(), "wait service exit");
    main_tcp_server_close();
    future.wait();
    RCLCPP_INFO(node->get_logger(), "shutdown");
    rclcpp::shutdown();
    return 0;
}
